Lab. 08 - SLAM Wizyjny (ORB-SLAM3) cz. 2

1. Cel zajęć

Celem zajęć jest uruchomienie lokalizacji 3D z wykorzystaniem systemu ORB-SLAM3 na danych z eksperymentów przeprowadzonych na terenie Politechniki Poznańskiej i ocena jakości działania systemu.

2. Przygotowanie środowiska

Na dzisiejszych zajęciach będzie wykorzystywany obraz z zajęć poprzednich.

Korzystanie z kontenera

Po każdym ponownym uruchomieniu komputera, proszę pamiętać o wywoływaniu:

xhost +local:root

Nowy terminal można dołączyć do kontenera korzystając z polecenia:

docker exec -it ARM_07 bash

ROS_DOMAIN_ID

W przypadku pracy na wielu komputerach w tej samej sieci lokalnej (eduroam też może tego wymagać), konieczne może być ustawienie zmiennej środowiskowej ROS_DOMAIN_ID na różnych wartościach. Należy wybrać dowolną liczbę całkowitą z zakresu 0-100. Wartość tę należy ustawić w pliku ~/.bashrc:

export ROS_DOMAIN_ID=10  # przykładowa wartość

lub bezpośrednio w terminalu (wtedy będzie obowiązywać tylko w tym terminalu):

export ROS_DOMAIN_ID=10  # przykładowa wartość

Sugerowane jest użycie dwóch ostatnich cyfr numeru IP komputera jako wartości ROS_DOMAIN_ID, aby uniknąć konfliktów z innymi użytkownikami w sieci. Aby sprawdzić swój adres IP, można użyć polecenia:

ip addr show

adres IP będzie widoczny przy interfejsie sieciowym, np. wlan0 lub eth0.

Wygodne może być dodanie tego exportu do bashrc:

echo 'export ROS_DOMAIN_ID=<wybrana_wartość>' >> ~/.bashrc 

3. Uruchomienie ORB-SLAM3 na danych z PP

Przed przystąpieniem do tego kroku konieczne będą dane z DGPS, które powstały w ramach poprzednich zajęć. Jeżeli ktoś z jakiegoś powodu nie otrzymał własnego wyniku, proszę od kogoś pożyczyć plik dgps.txt. Oczywiście proszę tego pliku nie przesyłać jako rozwiązanie zadania domowego z poprzednich zajęć.

Przygotowanie kalibracji układu stereowizyjnego

Każde uruchomienie systemu na własnej konfiguracji kamer wymaga podania parametrów użytego układu kamer. W naszym przypadku chcemy tę konfigurację umieścić w pliku PP.yaml, który znajduje się pod ścieżką /arm_ws/src/orbslam3_ros2/config/stereo. Wartości parametrów tym pliku są aktualne dla innego układu stereowizyjnego, w ramach zajęć konieczna bedzie modyfikacja tych parametrów: - Camera.width - 720 - Camera.height - 540 - LEFT.height - 540 - LEFT.width - 720 - RIGHT.height - 540 - RIGHT.width - 720

Ponadto, aby poznać niektóre parametry konieczna będzie kalibracja układu. W tym przypadku zostało to już zrealizowane (wykorzystując pakiet kalibr). Uzyskano następujący wynik:

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.23248459123344795, 0.08191912671589296, 7.839314739418238e-05, 4.2075874672940684e-05]
  distortion_model: radtan
  intrinsics: [589.7363083173951, 590.0729541236071, 369.91183932674056, 298.34374442536097]
  resolution: [720, 540]
  rostopic: /pylon_stereo_node/left/image_raw
cam1:
  T_cn_cnm1:
  - [0.9997283077554007, 0.02320592542857902, 0.0021899081072863703, -0.32185797263546406]
  - [-0.0232254786906617, 0.9996863295720175, 0.00937121157638949, 0.0074867236444067856]
  - [-0.001971753560855992, -0.00941952715496201, 0.9999536912757833, -0.002949869596374046]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.2303444874136257, 0.07876654742388302, 9.186197190170094e-05,
    -0.0005160611286370294]
  distortion_model: radtan
  intrinsics: [589.5069022640636, 589.958834706832, 379.29038618376575, 283.91319737264314]
  resolution: [720, 540]
  rostopic: /pylon_stereo_node/right/image_raw

Część tych parametrów można przepisać bezpośrednio do pliku PP.yaml (Camera.fx, Camera.fy, Camera.cx, Camera.cy, macierze D i K). Natomiast wartości macierzy R oraz P, a także Camera.bf można poznać dopiero po wykorzystaniu narzędzia stereoRectify z biblioteki OpenCV. W tym celu można skorzystać z przygotowanego skryptu. Znajduje się on w folderze scripts paczki orbslam3_ros2. Należy uzupełnić ten skrypt odpowiednimi wartościami.

Wartość Camera.bf znajdą Państwo jako wartość bezwzględną z wartości w macierzy P2 w prawym, górnym narożniku.

Uwaga! Należy zwrócić uwagę czy wynik kalibracji to układ prawej kamery w układzie lewej kamery czy też lewej w układzie prawej.

PUT Car

Po poprawnej kalibracji możliwe jest uruchomienie ORB-SLAM3 na danych z PP.

Baga można uruchomić poleceniem:

ros2 bag play -r 0.25 bags/put_car -p --remap /pylon_stereo_node/left/image_raw:=/camera/left /pylon_stereo_node/right/image_raw:=/camera/right

Uwaga! System ORB-SLAM3 jest dość wymagający obliczeniowo, więc przypominam o parametrze -r przy rosbag play, który umożliwia zwolnienie odtwarzania danych z rosbaga.

a ORB-SLAM3:

ros2 run orbslam3 stereo /arm_ws/src/orbslam3_ros2/vocabulary/ORBvoc.txt /arm_ws/src/orbslam3_ros2/config/stereo/PP.yaml true

Sugeruję użyć opcji Follow Camera, aby nawigowanie podczas tworzonej trajektorii było łatwiejsze.

Po zakończeniu działania systemu należy zapisać trajektorię klikając przycisk Stop w oknie ORB-SLAM3: Map Viewer.

4. Ewaluacja ORB-SLAM3

Ostatnim krokiem jest ewaluacja uzyskanego wyniku. Do ewaluacji wykorzystamy narzędzie evaluate_ate.py wraz z dodatkowym skryptem associate.py, które znajdą Państwo w katalogu /arm_ws/src/orbslam3_ros2/scripts. Korzystamy z niego następująco: shell cd /arm_ws/src/orbslam3_ros2/scripts/ python3 evaluate_ate.py DGPS_PATH ORBSLAM_PATH --verbose --plot trajs.png gdzie w miejsce DGPS_PATH podajemy ścieżkę do pliku dgps.txt, a w miejsce ORBSLAM_PATH ścieżkę do trajektorii zwróconej przez ORB-SLAM3.

Uzyskany wynik proszę zapisać. Narzędzie wylicza najlepsze nałożenie dwóch podanych trajektorii (brak konieczności znania transformacji pomiędzy układem DGPS i układem kamery) generując wykres porównujący obie trajektorie oraz wyliczający błąd.

Nałożone trajektorie można podejrzeć poleceniem:

apt install viewnior
viewnior trajs.png

5. Zadanie do samodzielnej realizacji

Państwa zadaniem jest umieszczenie na platformie e-kursy: - pliku PP.yaml (z rozszerzeniem .txt) - zdjęcia wygenerowanego narzędziem evaluate_ate.py - uzyskane wyniki liczbowe z evaluate_ate.py. System nie jest deterministyczny, więc wyniki będą prawdopodobnie różne dla każdego z Państwa.

_{Autorzy: Michał Nowicki, Kamil Młodzikowski}